using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints.Contact
{
    // 有关约束的深入解释,请查看Inequality1DOF.cs实现。
    // 为简洁起见,在其他实现中省略了细节。

    public struct TwistFrictionProjection
    {
        // 雅可比和惯性与其他约束共享。
        public Vector<float> EffectiveMass;
    }

    /// <summary>
    /// 处理切线摩擦实现。
    /// </summary>
    public static class TwistFriction
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide angularJacobianA,
            out TwistFrictionProjection projection)
        {
            // 计算有效质量矩阵贡献。扭曲约束没有线性贡献。
            // 请注意,尽管角度JacobianB=-angularJacobianA,但我们对两者都使用角度JacobianA(即法线)。这很好--J*M*JT无论如何都会是正数。
            Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularA);
            Symmetric3x3Wide.VectorSandwich(angularJacobianA, inertiaB.InverseInertiaTensor, out var angularB);

            // 没有软化;此约束在设计上是刚性的。(它确实支持最大力,但这与适当的阻尼比/固有频率不同。)
            // 请注意,我们必须警惕具有无限惯性的两个物体。这是一个有效的状态！
            // (我们不必对线性雅克比的约束进行这样的保护;动力学物体不能有零*质量*。)
            // (还要注意,这里不需要epsilon.用户不应该将他们的惯性设置为导致问题所需的荒诞值。
            // 无效条件不能动态出现。)
            var inverseEffectiveMass = angularA + angularB;
            var inverseIsZero = Vector.Equals(Vector<float>.Zero, inverseEffectiveMass);
            projection.EffectiveMass = Vector.ConditionalSelect(inverseIsZero, Vector<float>.Zero, Vector<float>.One / inverseEffectiveMass);

            // 请注意,摩擦约束没有偏移速度。他们的目标是零速度。
        }

        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref Vector<float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA);
            Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
            Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaB.InverseInertiaTensor, out var worldCorrectiveVelocityB);
            Vector3Wide.Add(wsvA.Angular, worldCorrectiveVelocityA, out wsvA.Angular);
            Vector3Wide.Subtract(wsvB.Angular, worldCorrectiveVelocityB, out wsvB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref accumulatedImpulse, ref wsvA, ref wsvB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection,
            ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Vector<float> maximumImpulse,
            ref Vector<float> accumulatedImpulse, out Vector<float> correctiveCSI)
        {
            Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA);
            Vector3Wide.Dot(wsvB.Angular, angularJacobianA, out var negatedCSVB);
            var negatedCSI = (csvA - negatedCSVB) * projection.EffectiveMass; // 因为没有偏见或软弱会给我们带来负面影响,所以当我们施加到累积的冲动时,我们就会这样做。

            var previousAccumulated = accumulatedImpulse;
            accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negatedCSI));

            correctiveCSI = accumulatedImpulse - previousAccumulated;

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref TwistFrictionProjection projection,
            ref Vector<float> maximumImpulse, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref wsvB, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(ref angularJacobianA, ref inertiaA, ref inertiaB, ref correctiveCSI, ref wsvA, ref wsvB);

        }

    }
}
